// This is the main DLL file.

#include "stdafx.h"
#include <memory>
#include <math.h>
#include <ppl.h>
#include "NativeStructure.h"

using namespace Concurrency;

template<typename T>
T min(T v1, T v2)
{
	if (v1<v2) return v1; else return v2;
}

template<typename T>
T max(T v1, T v2)
{
	if (v1>v2) return v1; else return v2;
}

void svd(int m, int n, double **a, double **p, double *d, double **q);

Matrix3 ComputeHomography(float* x1, float *y1, float *x2, float *y2)
{
	double *a[10], *p[10];
	double *vp[9];
	double w[9];
	double v[9][9];
	double A[12][9];
	double P[12][9];
	memset(A, 0, sizeof(double)*10*9);
	for (int i =0; i<10; i++)
	{
		a[i] = A[i];
		p[i] = P[i];
	}
	for (int i =0; i<9; i++)
		vp[i] = v[i];
    for(int ii = 0; ii<5; ii++) 
	{
		int i = ii%4;
       A[ii*2][0] = -x1[i];
	   A[ii*2][1] = -y1[i];
	   A[ii*2][2] = -1.0f;
	   A[ii*2][6] = x1[i] * x2[i];
	   A[ii*2][7] = y1[i] * x2[i];
	   A[ii*2][8] = x2[i];
	   A[ii*2+1][3] = -x1[i];
	   A[ii*2+1][4] = -y1[i];
	   A[ii*2+1][5] = -1.0f;
	   A[ii*2+1][6] = x1[i] * y2[i];
	   A[ii*2+1][7] = y1[i] * y2[i];
	   A[ii*2+1][8] = y2[i];
	}
	
	
	svd(10, 9, a, p, w, vp);
	int minid = 0;
	for (int i = 1; i<8; i++)
		if (w[i]<w[minid])
			minid = i;
    Matrix3 rs;
	for (int i = 0; i<9; i++)
		rs.m[i] = (float)v[i][minid];

	return rs;
}
struct Color
{
	unsigned char R,G,B,A;
	Color()
	{}
	Color(unsigned int c)
	{
		R = c>>24;
		G = (c>>16)&255;
		B = (c>>8)&255;
		A = c&255;
	}
};
inline int SampleTexture(::Image image, Float2 & _uv)
{
	Float2 uv = _uv;
	uv.x *= image.Width;
	uv.y *= image.Height;
	int i0 = (int)(uv.x);
	if (i0 < 0) return 0;
	if (i0 >= image.Width) return 0;
	int i1 = i0+1>=image.Width?0:i0+1;
	int j0 = (int)(uv.y);
	if (j0 < 0) return 0;
	if (j0 >= image.Height) return 0;
	int j1 = j0+1>=image.Height?0:j0+1;
		::Color c1,c2,c3,c4;
	c1 = ::Color(image.Pixels[j0 * image.Width + i0]);
	c2 = ::Color(image.Pixels[j0 * image.Width + i1]);
	c3 = ::Color(image.Pixels[j1 * image.Width + i0]);
	c4 = ::Color(image.Pixels[j1 * image.Width + i1]);
	::Color ci0, ci1;
	float it = uv.x - i0;
	float jt = uv.y - j0;
	float invIt = 1.0f - it;
	float invJt = 1.0f - jt;
	ci0.R = (unsigned char)(c1.R * invIt + c2.R * it);
	ci0.G = (unsigned char)(c1.G * invIt + c2.G * it);
	ci0.B = (unsigned char)(c1.B * invIt + c2.B * it);
	ci0.A = (unsigned char)(c1.A * invIt + c2.A * it);
	ci1.R = (unsigned char)(c3.R * invIt + c4.R * it);
	ci1.G = (unsigned char)(c3.G * invIt + c4.G * it);
	ci1.B = (unsigned char)(c3.B * invIt + c4.B * it);
	ci1.A = (unsigned char)(c3.A * invIt + c4.A * it);
	::Color c;
	c.R = (unsigned char)(ci0.R * invJt + ci1.R * jt);
	c.G = (unsigned char)(ci0.G * invJt + ci1.G * jt);
	c.B = (unsigned char)(ci0.B * invJt + ci1.B * jt);
	c.A = (unsigned char)(ci0.A * invJt + ci1.A * jt);
	return (c.R<<24) + (c.G<<16) + (c.B<<8) + c.A;
}

void Rectify(::Image im, ::Image rs, float * x1, float * y1)
{
	float x2[] = {0.0f, 0.0f, 1.0f, 1.0f};
	float y2[] = {0.0f, 1.0f, 1.0f, 0.0f};
	for (int i = 0; i<4; i++)
		y1[i] = 1.0f-y1[i];
	auto H = ComputeHomography(x2, y2, x1, y1);
	parallel_for(0, rs.Width, [&](int i)
	{
		float x = i/(float)(rs.Width-1);
		for (int j = 0; j<rs.Height; j++)
		{
			float y = j/(float)(rs.Height-1);
			auto p = H.Transform(x, y);
			rs.Pixels[j*rs.Width + i] = SampleTexture(im, p);
		}
	});
}